#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "robot_msgs/action/robot_goto.hpp"
#include "robot_msgs/msg/robot_status.hpp"

class ClientNode : public rclcpp::Node
{
  private:
	rclcpp_action::Client<robot_msgs::action::RobotGoto>::SharedPtr client;
	std::shared_ptr<rclcpp::TimerBase> timer;
	rclcpp_action::Client<robot_msgs::action::RobotGoto>::SendGoalOptions options;
	robot_msgs::action::RobotGoto::Goal goal;
	rclcpp::CallbackGroup::SharedPtr callback_group;

	// called when the goal got a response(accpetd or not)
	void goalresponse_callback(
		rclcpp_action::ClientGoalHandle<robot_msgs::action::RobotGoto>::SharedPtr goalhandle)
	{
		if (goalhandle->get_status())
		{
			RCLCPP_INFO(this->get_logger(), "goal accepted by server");
		}
		else
		{
			RCLCPP_INFO(this->get_logger(), "goal rejected by server");
		}
	}

	// called when the goal is finished with a result
	void result_callback(
		const rclcpp_action::ClientGoalHandle<robot_msgs::action::RobotGoto>::WrappedResult
			&result)
	{
		auto res = result.result.get();
		RCLCPP_INFO(this->get_logger(), "goal finished with a result(x: %f, y: %f, c: %d)",
					res->pos.position.x, res->pos.position.y, (int)result.code);
		// restart action later
		goal.goal.position.x = std::rand() % 200 - 100;
		goal.goal.position.y = std::rand() % 200 - 100;
		goal.goal.position.z = 0;
		RCLCPP_INFO(this->get_logger(), "next goal: x: %f, y: %f", goal.goal.position.x,
					goal.goal.position.y);
		rclcpp::Rate(std::chrono::milliseconds(3000)).sleep();
		this->client->async_send_goal(goal, options);
	}

	// called after a cancel request is resolved by server
	void cancelled_callback(
		rclcpp_action::Client<robot_msgs::action::RobotGoto>::CancelResponse::SharedPtr
			response)
	{
		if (response->return_code == action_msgs::srv::CancelGoal_Response::ERROR_NONE)
		{
			RCLCPP_INFO(this->get_logger(), "successfully cancelled previous goal");
		}
		else
		{
			RCLCPP_INFO(this->get_logger(), "unexpected cancel response: %d",
						(int)response->return_code);
		}
		goal.goal.position.x = std::rand() % 200 - 100;
		goal.goal.position.y = std::rand() % 200 - 100;
		goal.goal.position.z = 0;
		RCLCPP_INFO(this->get_logger(), "next goal: x: %f, y: %f", goal.goal.position.x,
					goal.goal.position.y);
		rclcpp::Rate(std::chrono::milliseconds(3000)).sleep();
		this->client->async_send_goal(goal, options);
	}
	// called every time the server send a feedback
	void feedback_callback(
		rclcpp_action::ClientGoalHandle<robot_msgs::action::RobotGoto>::
			SharedPtr, // access this handle can cause deadlock due to handle_mutex_
		const std::shared_ptr<const robot_msgs::action::RobotGoto::Feedback> feedback)
	{
		RCLCPP_INFO(this->get_logger(), "feedback: x: %f, y: %f, t: %d",
					feedback->pose.position.x, feedback->pose.position.y, feedback->cycle);
		if (feedback->cycle >= 50)
		{
			RCLCPP_INFO(this->get_logger(), "cycle count reach 50, cancel goal");

			this->client->async_cancel_all_goals(); //to avoid ClientGoalHandle
			//	std::bind(&ClientNode::cancelled_callback, this, std::placeholders::_1));
			RCLCPP_INFO(this->get_logger(), "sent goal cancel request");
		}
	}

  public:
	ClientNode(std::string name) : Node(name)
	{
		client = rclcpp_action::create_client<robot_msgs::action::RobotGoto>(this, "RobotGoto");
		options.goal_response_callback =
			std::bind(&ClientNode::goalresponse_callback, this, std::placeholders::_1);
		options.result_callback =
			std::bind(&ClientNode::result_callback, this, std::placeholders::_1);
		options.feedback_callback = std::bind(&ClientNode::feedback_callback, this,
											  std::placeholders::_1, std::placeholders::_2);
		RCLCPP_INFO(this->get_logger(), "checking for action server...");
		while (!client->wait_for_action_server(std::chrono::seconds(1)))
		{
			RCLCPP_INFO(this->get_logger(), "action server now offline, still waiting...");
		}
		goal.goal.position.x = 0;
		goal.goal.position.y = 0;
		goal.goal.position.z = 0;
		client->async_send_goal(goal, options);
		RCLCPP_INFO(this->get_logger(), "first action request sent");
	}
};

int main(int argc, char **argv)
{
	rclcpp::init(argc, argv);
	auto node = std::make_shared<ClientNode>("client_node");
	rclcpp::spin(node);
	rclcpp::shutdown();
	return 0;
}
